#ifndef __PLANNING_BASE_H__
#define __PLANNING_BASE_H__

#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
#include <mc_core/common/core.h>
#include <mc_core/utils/graphis_utils.h>
#include <mc_core/utils/math_utils.h>
#include <mc_core/msg_convert/msg_convert.h>
#include <mc_map/map_base.h>

// 路径平滑
#include <mc_path_planner/path_smoother/five_point_gradient.h>

using namespace std;

enum PlannerOption{
    Astar = 1,
    Dstar_Lite = 2
};


class PlannerBase{
    public:
        using Point2i = mc::Point2i;
        using Point2f = mc::Point2f;
    
    public:
        PlannerBase(boost::shared_ptr<MapBase> map_handler_ptr);
        ~PlannerBase();

        // 路径生成 虚函数 对外统一接口
        virtual mc_status pathGenerate(Point2i start, Point2i goal, vector<Point2i>& points) = 0;

        // 路径规划 对外统一接口 ROS
        mc_status pathPlanning(geometry_msgs::Pose start, geometry_msgs::Pose goal, 
            vector<geometry_msgs::Pose>& poses);    

        // 规划类型转字符串
        string toString(PlannerOption option);
        // 生成路径方向
        vector<geometry_msgs::Pose> genPathDirection(vector<geometry_msgs::Pose>& poses, 
            geometry_msgs::Pose goal);
    
    protected:
        string frameId_;
        boost::shared_ptr<MapBase> mapHandlerPtr_;
        boost::shared_ptr<SmoothBase> smoothPathPtr_;
    private:
        ros::NodeHandle nh_;
        ros::Publisher pathPub_;
        ros::Publisher pathOriginPub_;
        ros::Publisher startPosePub_;
        ros::Publisher goalPosePub_;
        
};

#endif